
/**
 * @file chassis.c
 * @brief 底盘控制模块实现文件
 *        包含底盘模式控制、运动解算、状态管理等核心逻辑
 * @note 在函数数组chassis->ctrl_func后加入对应函数，并且在类型Chasis_Mode_Enum_Type后加入对应名称缩写，最后调用函数数组，下标是Chasis_Mode_Enum_Type中的模式类型
 * @attention[0]第[1]默认停止和恢复正常，配合状态检测＋！
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-08
 * 
 * @copyright Copyright (c) 2025  邱伟凡
 * 
 * @par 修改日志:
 *   2025-08-08: 初始版本创建
 *	 2025-08-27: 添加static关键字,合并chassis_task.c,添加轮腿适配
 * @bug 待解决问题:
 *   - 舵轮急刹时速度突变导致舵轮急刹问题
 */
#include "QWF_Custom_Ctrl.h"
#include "QWF_Include.h"
#include "QWF_Math.h"
#include "QWF_Usart.h"
#include "Filter.h"
#include "state.h"

#include "ctrl.h"
#include "chassis.h"
#include "pid.h"
#include "judge.h"
#include "power_ctrl.h"
#include "freertos.h"
#include "task.h"	
/*——————底盘参数(单位:米)——————*/
#if Chassis_Wheel_Type==Omni_Wheel
#define mecanum_r				(0.025f) 
#elif Chassis_Wheel_Type==Mecanum_Wheel
#define omni_r					(0.025f) 
#elif Chassis_Wheel_Type==Helf_Helm_Wheel
///< 全向轮直径（单位：米）
#define omni_r          		(0.025f)    //全向轮直径,
///< 舵轮直径（单位：米）
#define	helm_r					(0.03f)	    //舵轮直径，
///< 底盘X轴方向两轮中心间距（单位：米）
#define D_X                     (4.38406f)     //底盘X轴上两轮中心的间距
///< 底盘Y轴方向两轮中心间距（单位：米）
#define D_Y                     (4.38406f)     //底盘Y轴上两轮中心的间距
#define D_gm6020				(0.667f)				
#define r_x						(1)
#define r_y						(1)

#define r_helm_wheel			(0.116f)
#define helm_ratio				(17.0f/268)//
#define r_omni_wheel			(0.152f)
#define omni_ratio				(187.0f/3591)//

#define omni_v_to_helm_v		((r_helm_wheel/helm_ratio)/(r_omni_wheel/omni_ratio))
/********************舵轮示意图***************************
						F
	y				  G1 C4								。->w+
	|                 <-|								|
	|               	|
—— ——x		L C7—— ——C1	R                          。—>gm6020角速度正
	|               	|                                  |
	|               	|->
					  G4 C2
						B
						默认初始状态:两个3508普股朝外面
******************************************************/

/********************脉轮示意图*******************
                         |
                    lf-------rf
    y               |\       /|               /
    |               | \     / |              /
    |               |  \   /  |             /  Wheel_Centrl_Angle
---- -------x       |         |   -------> .---------
    |               |  /   \  |            \
    |               | /     \ |             \
                    |/       \|              \
                    lb-------rb

**********************************************/
#elif Chassis_Wheel_Type==Leg_Wheel
#endif

/**
 * @brief 半舵轮底盘输入转换函数
 *        将遥控器/键盘输入转换为底盘X/Y轴目标速度
 *        支持RC（遥控器）和KM（键鼠）两种控制模式
 */
static void rc_chance(void){
	/*通过遥控器控制机体整体期望状态
		90
		+|+
	0---	---180/-180
		-|-
		-90
	*/
	if(ctrl->mode==RC){//遥控器
		        // 遥控器右摇杆上下通道（CH2）转换为X轴目标速度（反向处理）
        chassis->sys.v.x.target[now]=-ctrl->RC[now].ch[2]/660.0f*8000;
		        // 遥控器右摇杆左右通道（CH3）转换为Y轴目标速度（反向处理）
        chassis->sys.v.y.target[now]=-ctrl->RC[now].ch[3]/660.0f*8000;
	}
	else if(ctrl->mode==KM){
		
//		chassis->sys.v.x.target[now]=QWF_Ramp((-(rc_key(time).d-rc_key(time).a)*chassis_normal_v_max),chassis->sys.v.x.target[now],15);
//		chassis->sys.v.y.target[now]=QWF_Ramp((-(rc_key(time).w-rc_key(time).s)*chassis_normal_v_max),chassis->sys.v.y.target[now],15);		
		chassis->sys.v.y.target[now]=(-(ctrl->KM[now].key.w-ctrl->KM[now].key.s)*8000);		
		chassis->sys.v.x.target[now]=(-(ctrl->KM[now].key.d-ctrl->KM[now].key.a)*8000);
		
	}
	        // 计算底盘合速度（X/Y轴速度的矢量和）
        chassis->sys.v.all.target[now]=sqrt(chassis->sys.v.x.target[now]*chassis->sys.v.x.target[now]+chassis->sys.v.y.target[now]*chassis->sys.v.y.target[now]);
//	if(chassis->sys.v.y.target[now]*chassis->sys.v.y.target[last]!=-1){
//	chassis->sys.v.y.target[now]=QWF_Ramp(chassis->sys.v.y.target[now],chassis->sys.v.y.target[last],15);
//	}
//	chassis->sys.v.y.target[last]=chassis->sys.v.y.target[now];	
//	sys_v=sqrt(chassis->sys.v.x.target[now]*chassis->sys.v.x.target[now]+chassis->sys.v.y.target[now]*chassis->sys.v.y.target[now]);
//	chassis_w_max=2200;
//	chassis_w_max=8000;	
//		sys_angle=(sys_v)?(chassis->sys.v.y.target[now]?	atan((float)chassis->sys.v.x.target[now]/(float)chassis->sys.v.y.target[now])/PI*180:	(chassis->sys.v.x.target[now]>0?-90:90)):sys_angle;
}
/**
 * @brief 底盘主任务函数
 *        周期执行底盘控制逻辑（1ms周期）
 *        包含输入处理、模式控制、PID计算、功率限制、电机数据发送
 * @param argument FreeRTOS任务参数（未使用）
 * @return 无（任务函数不返回）
 */
void chassis_task(void const * argument)
{
	static TickType_t xLastWakeTime;  // FreeRTOS任务唤醒时间戳（用于周期控制）
    const TickType_t xPeriod = pdMS_TO_TICKS(1); // 任务执行周期（1ms）
    // 初始化唤醒时间
    xLastWakeTime = xTaskGetTickCount();  // 初始化唤醒时间戳
	chassis->pid=chassis_pid;
	while(1){
		rc_chance();  // 执行输入转换（遥控器/键盘→目标速度）
		chassis->ctrl_func[chassis->mode[now]!=chassis_stop]();  // 执行非停止模式控制函数
		if(ctrl->type!=Null_Ctrl)
			chassis->ctrl_func[chassis->mode[now]]();
		else
			chassis->ctrl_func[chassis->mode[now]=chassis_stop]();
		        // 舵轮角速度PID计算（目标值→输出量）
       
		/*-----------------------发送数据给VOFA---------------------------------------------*/
//		Add_Data_To_Vofa(chassis_pid->f.helm.angle.target[now],f_angle_target);  // 前舵轮目标角度发送至VOFA调试工具
//		Add_Data_To_Vofa(chassis_pid->f.helm.angle.measure[now],f_angle_meas);
//		Add_Data_To_Vofa(chassis_pid->b.helm.angle.target[now],b_angle_target)
//		Add_Data_To_Vofa(chassis_pid->b.helm.angle.measure[now],b_angle_meas);
//		Add_Data_To_Vofa(chassis_pid->f.wheel.v.target[now],f_v_target)
//		Add_Data_To_Vofa(chassis_pid->f.wheel.v.measure[now],f_v_meas);
//		Add_Data_To_Vofa(chassis_pid->b.wheel.v.target[now],b_v_target)
//		Add_Data_To_Vofa(chassis_pid->b.wheel.v.measure[now],b_v_meas);	
//		Add_Data_To_Vofa(chassis_pid->l.wheel.v.target[now],l_v_target)
//		Add_Data_To_Vofa(chassis_pid->l.wheel.v.measure[now],l_v_meas);
//		Add_Data_To_Vofa(chassis_pid->r.wheel.v.target[now],r_v_target)
//		Add_Data_To_Vofa(chassis_pid->r.wheel.v.measure[now],r_v_meas);			
		vTaskDelayUntil(&xLastWakeTime, xPeriod);  // 任务周期延时（确保1ms执行一次）	
	}
}

/*********************************************云台映射底盘*************************************************/
/**
 * @brief 云台角度映射到底盘坐标系
 *        将遥控器输入的速度指令从云台坐标系转换到底盘坐标系
 *        用于实现底盘跟随云台方向运动
 * @note 低层核心函数，使用static提高运行效率
 */
static void Gimbal_Mapping(void){
    // 暂存当前的vy速度
    float vy =  chassis->sys.v.y.target[now]; 
    // 暂存当前的vx速度
    float vx =  chassis->sys.v.x.target[now]; 
    // 根据角度差计算新的vy速度（左右方向）
    chassis->sys.v.y.target[now] = +vy * arm_cos_f32((chassis->part.yaw.motor.rx_meas.angle[0]-chassis->part.yaw.target)*PI/4092)  + vx * arm_sin_f32((chassis->part.yaw.motor.rx_meas.angle[0]-chassis->part.yaw.target)*PI/4092) ; 
    // 根据角度差计算新的vx速度（前后方向）                                                                                                   
    chassis->sys.v.x.target[now] = -vy * arm_sin_f32((chassis->part.yaw.motor.rx_meas.angle[0]-chassis->part.yaw.target)*PI/4092)  + vx * arm_cos_f32((chassis->part.yaw.motor.rx_meas.angle[0]-chassis->part.yaw.target)*PI/4092) ; 
}
// 检查是否定义了底盘轮子类型，如果未定义则报错
#ifndef Chassis_Wheel_Type
	#error "Undefine 'Chassis_Wheel_Type'"  // 未定义底盘类型时报错
#elif ((Chassis_Wheel_Type<Chassis_Wheel_Type_Min)||(Chassis_Wheel_Type>Chassis_Wheel_Type_Max))
	#error "Error state of Chassis_Wheel_Type'"  // 底盘类型非法时报错
#endif
/**
 * @brief 底盘运动解算函数
 *        根据目标速度(vx, vy, w)计算各轮子的目标角度和速度
 *        支持半舵轮、全向轮、麦克纳姆轮等不同底盘类型
 * @note 低层核心函数，使用static提高运行效率
 */
static void Wheel_Solution(void){ // 轮子解算，正常行走
#if Chassis_Wheel_Type == Helf_Helm_Wheel // 半舵底盘解算
    /***************************************** 前轮 ********************************************************/
    if (chassis->sys.v.x.target[now] || chassis->sys.v.x.target[now]||chassis->sys.v.w.target[now] ){
        chassis->part.f.helm.target.angle = To_0_T(atan2(chassis->sys.v.y.target[now], -(chassis->sys.v.x.target[now] - chassis->sys.v.w.target[now] * r_y)) * 4096 / PI, 8192); 
	}
	
    if(QWF_Esp(find_min_angle_diff(chassis->part.f.helm.motor.rx_meas.angle[2], chassis->part.f.helm.target.angle			  , 8192), 0)\
    <= QWF_Esp(find_min_angle_diff(chassis->part.f.helm.motor.rx_meas.angle[2], chassis->part.f.helm.target.angle + 8192 / 2, 8192), 0)){// 正常转动角度差小
		chassis->part.f.wheel.target.signed_v=1;
        chassis->part.f.helm.target.angle = find_min_angle(chassis->part.f.helm.target.angle, chassis->part.f.helm.motor.rx_meas.angle[2], 8192); 
    }
    else{  // 扣半圈转动角度差小
		chassis->part.f.wheel.target.signed_v=-1;
        chassis->part.f.helm.target.angle = find_min_angle(chassis->part.f.helm.target.angle+8192/2, chassis->part.f.helm.motor.rx_meas.angle[2], 8192); // 目标角度加上半圈
    }
	chassis->part.f.wheel.target.v=chassis->part.signed_all*chassis->part.f.wheel.target.signed_v*sqrt((chassis->sys.v.x.target[now] - chassis->sys.v.w.target[now] * r_y) * (chassis->sys.v.x.target[now] - chassis->sys.v.w.target[now] * r_y) + chassis->sys.v.y.target[now] * chassis->sys.v.y.target[now]);
    chassis->part.f.helm.target.w = PID_Calculate(&chassis_pid->f.helm.angle, chassis->part.f.helm.motor.rx_meas.angle[2], chassis->part.f.helm.target.angle); 
    /*最后控制出来角度差肯定是0-90.故使用cos三次方函数对速度做衰减（以免太偏）*/
    chassis->part.f.wheel.target.v *= arm_cos_f32(chassis_pid->f.helm.angle.err[now] * PI / 4092.0f) * arm_cos_f32(chassis_pid->f.helm.angle.err[now] * PI / 4092.0f) * arm_cos_f32(chassis_pid->f.helm.angle.err[now] * PI / 4092.0f); 
    /******************************************后轮 **************************************/
    if (chassis->sys.v.y.target[now] || chassis->sys.v.x.target[now]||chassis->sys.v.w.target[now] )
        chassis->part.b.helm.target.angle = To_0_T(atan2(chassis->sys.v.y.target[now], -(chassis->sys.v.x.target[now] + chassis->sys.v.w.target[now] * r_y)) * 4096 / PI, 8192); 
    if (QWF_Esp(find_min_angle_diff(chassis->part.b.helm.motor.rx_meas.angle[2], chassis->part.b.helm.target.angle		   , 8192), 0) \
	 <= QWF_Esp(find_min_angle_diff(chassis->part.b.helm.motor.rx_meas.angle[2], chassis->part.b.helm.target.angle + 8192 / 2, 8192), 0)){// 正常转动角度差小 
		chassis->part.b.wheel.target.signed_v=-1;     
        chassis->part.b.helm.target.angle = find_min_angle(chassis->part.b.helm.target.angle, chassis->part.b.helm.motor.rx_meas.angle[2], 8192); 
    }
    else{// 扣半圈转动角度差小
		chassis->part.b.wheel.target.signed_v=1;
        chassis->part.b.helm.target.angle = find_min_angle(chassis->part.b.helm.target.angle+8192/2, chassis->part.b.helm.motor.rx_meas.angle[2], 8192); 
    }
	chassis->part.b.wheel.target.v=chassis->part.signed_all*chassis->part.b.wheel.target.signed_v*sqrt((chassis->sys.v.x.target[now] + chassis->sys.v.w.target[now] * r_y) * (chassis->sys.v.x.target[now] + chassis->sys.v.w.target[now] * r_y) + chassis->sys.v.y.target[now] * chassis->sys.v.y.target[now]);

    chassis->part.b.helm.target.w = PID_Calculate(&chassis_pid->b.helm.angle, chassis->part.b.helm.motor.rx_meas.angle[2], chassis->part.b.helm.target.angle); 
    /*最后控制出来角度差肯定是0-90.故使用cos三次方函数对速度做衰减（以免太偏）*/
    chassis->part.b.wheel.target.v *= arm_cos_f32(chassis_pid->b.helm.angle.err[now] * PI / 4096.0f) * arm_cos_f32(chassis_pid->b.helm.angle.err[now] * PI / 4096.0f) * arm_cos_f32(chassis_pid->b.helm.angle.err[now] * PI / 4096.0f); 
    /*************************** 左轮 ******************************************* */ 
    chassis->part.l.wheel.target.v = -1*chassis->part.signed_all *(chassis->sys.v.y.target[now] - chassis->sys.v.w.target[now] * r_x)*omni_v_to_helm_v; 
    /****************** 右轮************************* */
    chassis->part.r.wheel.target.v = 1 *chassis->part.signed_all* (chassis->sys.v.y.target[now] + chassis->sys.v.w.target[now] * r_x)*omni_v_to_helm_v; 
	PID_Calculate(&chassis_pid->f.helm.w,	chassis->part.f.helm.motor.rx_meas.rpm,	 chassis->part.f.helm.target.w);
	PID_Calculate(&chassis_pid->b.helm.w,	chassis->part.b.helm.motor.rx_meas.rpm,	 chassis->part.b.helm.target.w);
				//所有轮子pid计算
	PID_Calculate(&chassis_pid->f.wheel.v,	chassis->part.f.wheel.motor.rx_meas.rpm,  chassis->part.f.wheel.target.v);
	PID_Calculate(&chassis_pid->b.wheel.v,	chassis->part.b.wheel.motor.rx_meas.rpm,  chassis->part.b.wheel.target.v);
	PID_Calculate(&chassis_pid->l.wheel.v,	chassis->part.l.wheel.motor.rx_meas.rpm,  chassis->part.l.wheel.target.v);
	PID_Calculate(&chassis_pid->r.wheel.v,	chassis->part.r.wheel.motor.rx_meas.rpm,  chassis->part.r.wheel.target.v);

	Chassis_Power_Control();//功率控制	
	        // 前舵轮电机输出赋值（PID计算结果转换为16位整型）
    chassis->part.f.helm.target.out =(int16_t)chassis_pid->f.helm.w.output ;
	chassis->part.f.wheel.target.out=(int16_t)chassis_pid->f.wheel.v.output ;
	chassis->part.b.helm.target.out =(int16_t)chassis_pid->b.helm.w.output ;
	chassis->part.b.wheel.target.out=(int16_t)chassis_pid->b.wheel.v.output ;
	chassis->part.l.wheel.target.out=(int16_t)chassis_pid->l.wheel.v.output ;
	chassis->part.r.wheel.target.out=(int16_t)chassis_pid->r.wheel.v.output ;
	/*-----------------------发送数据给电机---------------------------------------------*/
	send_data_chassis_gm6020(f.helm); 
	send_data_chassis_gm6020(b.helm);
	send_data_chassis_c620	(f.wheel);
	send_data_chassis_c620	(b.wheel);	
	send_data_chassis_c620	(l.wheel);
	send_data_chassis_c620	(r.wheel);

#elif Chassis_Wheel_Type==Omni_Wheel||Chassis_Wheel_Type==Mecanum_Wheel // 全向轮和脉轮
    // 计算右前轮目标速度
    chassis->part.rf.target.v = (+chassis->sys.v.x.target[now] - chassis->sys.v.y.target[now] + chassis->sys.v.w.target[now] ) * 5; 
    // 计算左前轮目标速度
    chassis->part.lf.target.v = (+chassis->sys.v.x.target[now] + chassis->sys.v.y.target[now] + chassis->sys.v.w.target[now] ) * 5; 
    // 计算左后轮目标速度
    chassis->part.lb.target.v = (-chassis->sys.v.x.target[now] + chassis->sys.v.y.target[now] + chassis->sys.v.w.target[now] ) * 5; 
    // 计算右后轮目标速度
    chassis->part.rb.target.v = (-chassis->sys.v.x.target[now] - chassis->sys.v.y.target[now] + chassis->sys.v.w.target[now] ) * 5; 
	PID_Calculate(&chassis_pid->lf.v,	chassis->part.lf.motor.rx_meas.rpm,  chassis->part.lf.target.v);
	PID_Calculate(&chassis_pid->lb.v,	chassis->part.lb.motor.rx_meas.rpm,  chassis->part.lb.target.v);
	PID_Calculate(&chassis_pid->rf.v,	chassis->part.rf.motor.rx_meas.rpm,  chassis->part.rf.target.v);
	PID_Calculate(&chassis_pid->rb.v,	chassis->part.rb.motor.rx_meas.rpm,  chassis->part.rb.target.v);
	Chassis_Power_Control();//功率控制	
	chassis->part.lf.target.out=(int16_t)chassis_pid->lf.v.output ;
	chassis->part.lb.target.out=(int16_t)chassis_pid->lb.v.output ;
	chassis->part.rf.target.out=(int16_t)chassis_pid->rf.v.output ;
	chassis->part.rb.target.out=(int16_t)chassis_pid->rb.v.output ;
	send_data_chassis_gm6020(lf); 
	send_data_chassis_gm6020(lb);
	send_data_chassis_c620	(rf);
	send_data_chassis_c620	(rb);	

#endif
}
/**
 * @brief 底盘急停控制函数
 *        将所有运动目标置零，并关闭PID输出
 *        用于错误处理或紧急停止场景
 */
static void Chassis_Stop(void){// 停止底盘
#if Chassis_Wheel_Type == Helf_Helm_Wheel
	// 将底盘的角速度和线速度置为0
    chassis->sys.v.w.target[now] = chassis->sys.v.y.target[now] = chassis->sys.v.x.target[now] = 0; 
    // 如果四个轮向电机测量速度接近0，pid不输出
    if (QWF_Esp(chassis->part.f.wheel.motor.rx_meas.rpm, 0) <= 100)
        chassis_pid->f.wheel.v.mode.out_put=0; 
    if (QWF_Esp(chassis->part.b.wheel.motor.rx_meas.rpm, 0) <= 100)
        chassis_pid->b.wheel.v.mode.out_put=0; 
    if (QWF_Esp(chassis->part.l.wheel.motor.rx_meas.rpm, 0) <= 100)
        chassis_pid->l.wheel.v.mode.out_put=0; 
    if (QWF_Esp(chassis->part.r.wheel.motor.rx_meas.rpm, 0) <= 100)
        chassis_pid->r.wheel.v.mode.out_put=0; 
	chassis_pid->f.helm.w.mode.out_put		=0;
	chassis_pid->f.helm.angle.mode.out_put	=0;
	chassis_pid->b.helm.w.mode.out_put		=0;
	chassis_pid->b.helm.angle.mode.out_put	=0;
	//舵向电机直接不输出
#elif Chassis_Wheel_Type==Omni_Wheel||Chassis_Wheel_Type==Mecanum_Wheel
	
#elif Chassis_Wheel_Type==Leg_Wheel
	
#endif
    Wheel_Solution(); 
}
/**
 * @brief 底盘恢复正常模式
 *        重新启用各轴PID控制输出
 *        用于从停止状态恢复正常运行
 */
static void Chassis_Yes(void){// 正常
#if Chassis_Wheel_Type == Helf_Helm_Wheel
	chassis_pid->f.helm.w.mode.out_put=1;
	chassis_pid->f.helm.angle.mode.out_put=1;
	chassis_pid->b.helm.w.mode.out_put=1;
	chassis_pid->b.helm.angle.mode.out_put=1;		
	chassis_pid->f.wheel.v.mode.out_put=1;
	chassis_pid->b.wheel.v.mode.out_put=1;
	chassis_pid->l.wheel.v.mode.out_put=1;
	chassis_pid->r.wheel.v.mode.out_put=1;
#elif Chassis_Wheel_Type==Omni_Wheel||Chassis_Wheel_Type==Mecanum_Wheel
	
#elif Chassis_Wheel_Type==Leg_Wheel
	
#endif
}
/**
 * @brief 底盘普通控制模式
 *        仅控制线速度(vx, vy)，角速度置零
 *        用于基础运动测试场景
 */
static void Chassis_Normal(void){// 底盘不跟随
    // 将底盘角速度置为0
    chassis->sys.v.w.target[now] = 0; 
    // 进行轮子解算
    Wheel_Solution(); 
    return;
}
/**
 * @brief 底盘跟随云台模式
 *        通过PID控制使底盘角度与云台角度保持一致
 *        实现底盘自动对准云台方向
 */
static void Chassis_Follow(void){// 底盘跟随
    if (!qwf_state->head.motor.gimbal.yaw.state){
        // 计算底盘需要的角速度
		chassis->part.yaw.target=0;
	if((QWF_Esp(find_min_angle_diff(chassis->part.yaw.motor.rx_meas.angle[0],chassis->part.yaw.target			,8192),0))\
	 <=(QWF_Esp(find_min_angle_diff(chassis->part.yaw.motor.rx_meas.angle[0],chassis->part.yaw.target+8192/2	,8192),0))){
		chassis->part.yaw.target=find_min_angle(chassis->part.yaw.target	,chassis->part.yaw.motor.rx_meas.angle[0],8192); 
		chassis->part.signed_all=1;
	}
	else{
		chassis->part.yaw.target=find_min_angle(chassis->part.yaw.target+8192/2	,chassis->part.yaw.motor.rx_meas.angle[0],8192); 
		chassis->part.signed_all=-1;	
	}
    chassis->sys.v.w.target[now] = chassis->part.signed_all*PID_Calculate(&chassis_pid->special.follow,
                             chassis->part.yaw.motor.rx_meas.angle[0],
                            chassis->part.yaw.target); 	
        Gimbal_Mapping(); 				
        // 设置底盘角速度死区
        // 进行云台角度映射
    }
    // 进行轮子解算
    Wheel_Solution(); 
    return;
}
/**
 * @brief 小陀螺旋转模式
 *        以固定角速度绕自身中心旋转
 *        用于快速转向或躲避攻击场景
 */
static void Chassis_Top(void){// 小陀螺
    if (!qwf_state->head.motor.gimbal.yaw.state){
        chassis->sys.v.w.target[now] =-chassis->part.signed_all*chassis->sys.top.w; 
        Gimbal_Mapping(); 
//		chassis->sys.v.x.target[now]+=vx_;//偏心补偿
    }
    Wheel_Solution(); 
    return;
}
/**
 * @brief 飞坡特殊控制模式
 *        适配斜坡地形的运动控制逻辑
 *        暂与跟随模式共用实现，待优化
 */
static void Chassis_Fly_Slope(void){// 飞坡
    if (!qwf_state->head.motor.gimbal.yaw.state){
        // 计算底盘需要的角速度
		chassis->part.yaw.target=0;
	if((QWF_Esp(find_min_angle_diff(chassis->part.yaw.motor.rx_meas.angle[0],chassis->part.yaw.target			,8192),0))\
	 <=(QWF_Esp(find_min_angle_diff(chassis->part.yaw.motor.rx_meas.angle[0],chassis->part.yaw.target+8192/2	,8192),0))){
		chassis->part.yaw.target=find_min_angle(chassis->part.yaw.target	,chassis->part.yaw.motor.rx_meas.angle[0],8192); 
		chassis->part.signed_all=1;
	}
	else{
		chassis->part.yaw.target=find_min_angle(chassis->part.yaw.target+8192/2	,chassis->part.yaw.motor.rx_meas.angle[0],8192); 
		chassis->part.signed_all=-1;	
	}
    chassis->sys.v.w.target[now] = chassis->part.signed_all*PID_Calculate(&chassis_pid->special.follow,
                             chassis->part.yaw.motor.rx_meas.angle[0],
                            chassis->part.yaw.target); 	
        Gimbal_Mapping(); 				
        // 设置底盘角速度死区
        // 进行云台角度映射
	}
    return;
}

// 底盘控制函数数组，存储不同底盘模式对应的控制函数
///< 底盘控制实例初始化
///< 包含电机配置、模式数组、系统参数等核心数据
static chassis_t Chassis={
	.part={
		.signed_all=1,	
motor_str_config(yaw.motor	,yaw	,Only_rx)
#ifndef Chassis_Wheel_Type
	#error "Undefine 'Chassis_Wheel_Type'"  // 未定义底盘类型时报错
#elif ((Chassis_Wheel_Type<Chassis_Wheel_Type_Min)||(Chassis_Wheel_Type>Chassis_Wheel_Type_Max))
	#error "Error state of Chassis_Wheel_Type'"  // 底盘类型非法时报错
#endif	
#if	Chassis_Wheel_Type== Helf_Helm_Wheel
motor_str_config(f.helm.motor	,f_helm	,rx_and_tx)
motor_str_config(f.wheel.motor	,f_wheel,rx_and_tx)
motor_str_config(b.helm.motor	,b_helm	,rx_and_tx)
motor_str_config(b.wheel.motor	,b_wheel,rx_and_tx)			
motor_str_config(l.wheel.motor	,l_wheel,rx_and_tx)			
motor_str_config(r.wheel.motor	,r_wheel,rx_and_tx)		
#elif 	(Chassis_Wheel_Type==Omni_Wheel)||(Chassis_Wheel_Type==Mecanum_Wheel)
motor_str_config(lf.motor	,lf_wheel,rx_and_tx)
motor_str_config(lb.motor	,lb_wheel,rx_and_tx)			
motor_str_config(rf.motor	,rf_wheel,rx_and_tx)			
motor_str_config(rb.motor	,rb_wheel,rx_and_tx)	
#elif	Chassis_Wheel_Type==Leg_Wheel
					
#endif
	},
	.mode = chassis_stop,
	.sys.top.w=5000,
	.ctrl_func={Chassis_Stop, Chassis_Yes, Chassis_Normal, Chassis_Follow, Chassis_Top, Chassis_Fly_Slope},
};
chassis_t* chassis=&Chassis;
